#ifndef __UTILITY_FUNCTIONS__
#define __UTILITY_FUNCTIONS__

#include <iostream>

template <int d, typename position, typename index, typename element, 
  typename ordering, typename policy>
void
sift_down(position a, index i, element x, const ordering& less, policy& p) {
  //std::cout << " - before sift_down i,x: " << i << " " << x << std::endl;
  //show(a, a + p.size());
  //show(p.B.begin(), p.B.end());

  index j;
  index m = (p.last_leaf() >> 6) << 3;
  while (i < m) {
    const position ak = a + i;
    *(ak) = *(ak + 1);
    j = less(*(ak + 2), *(ak + 3)) ? 3 : 2;
    index k = j << 1;
    index k2 = k + 1;
    k = less(*(ak + k), *(ak + k2)) ? (i | k2) : (i | k);
    *(ak + 1) = *(ak + j);
    *(ak + j) = *(a + k);
    i = (k << 4) - ((k >> 3) << 6) - 48; 
    position ak2 = a + i;
    if(less(*(ak2), *(ak2 - 8))) i -= 8;
    if(less(*(a + i), x)) {
      if(less(*(a + k), x)) {
	*(ak + j) = *(ak + 1);
	if(less(*(ak + 1), x)) {
	  *(ak + 1) = *(ak); 
	  if(less(*(ak), x))	
	    *(ak) = x;
	  else
	    *(ak + 1) = x;	
	}
	else
	  *(ak + j) = x;
      }
      else 
	*(a + k) = x;       
      return;
    }
    else
      *(a + k) =  *(a + i);      	 
  }

  j = i | 4;
  if ((((j << 4) - (i << 3) - 56) > p.last_leaf()) && ((i | 7) <= p.last_leaf())) { 
    const position ak = a + i;
    *(ak) = *(ak + 1);
    j = less(*(ak + 2), *(ak + 3)) ? 3 : 2;
    index k = j << 1;
    index k2 = k + 1;
    k = less(*(ak + k), *(ak + k2)) ? (i | k2) : (i | k);
    *(ak + 1) = *(ak + j);
    *(ak + j) = *(a + k);

    if(less(*(a + k), x)) {
      *(ak + j) = *(ak + 1);
      if(less(*(ak + 1), x)) {
	*(ak + 1) = *(ak); 
	if(less(*(ak), x))	
	  *(ak) = x;
	else
	  *(ak + 1) = x;	
      }
      else
	*(ak + j) = x;
    }
    else 
      *(a + k) = x;       
    return;
  }


  while (p.last_child(i) <= p.last_leaf()) {
    j = p.top_all_present(a, i, less);
    if (less(x, *(a + j))) {
      p.update_all_present(a, i, *(a + j), less);
    }
    else {
      p.update_all_present(a, i, x, less);
      return;
    }
    i = j;
  }
  while(p.first_child(i) <= p.last_leaf()) {
    if(p.last_child(i) <= p.last_leaf())
      j = p.top_all_present(a, i, less);	
    else 
      j = p.top_some_absent(a, i, less);
    if (less(x, *(a + j))) {
      p.update_all_present(a, i, *(a + j), less);
      i = j;
    }
    else {
      p.update_all_present(a, i, x, less);
      return;
    }
  }    
  p.update_all_present(a, i, x, less);
  return;      

  //std::cout << "- after sift_down: " << std::endl;
  //show(a, a + p.size());
  //show(p.B.begin(), p.B.end()); 

}




template <int d, typename position, typename index, typename element, 
  typename ordering, typename policy>
void
sift_down0(position a, index i, element x, const ordering& less, policy& p) {
  //std::cout << " - before sift_down i,x: " << i << " " << x << std::endl;
  //show(a, a + p.size());
  //show(p.B.begin(), p.B.end());

  index j;
  index m = (p.last_leaf() >> 6) << 3;
  while (i < m) {
    const position ak = a + i;
    *(ak) = *(ak + 1);
    if(less(*(ak + 2), *(ak + 3))) {
      *(ak + 1) = *(ak + 3);
      if(less(*(ak + 6), *(ak + 7))) {
	i = ((i | 7) << 4) - (i << 3) - 48;
  	*(ak + 3) = *(ak + 7);
	position ak2 = a + i;
	if(less(*(ak2), *(ak2 - 8))) i -= 8;
	if(less(*(a + i), x)) {      
	  if(less(*(ak + 7), x)) {
	    *(ak + 3) = *(ak + 1);
	    if(less(*(ak + 1), x)) {
	      *(ak + 1) = *(ak); 
	      if(less(*(ak), x))	
		*(ak) = x;
	      else
		*(ak + 1) = x;	
	    }
	    else
	      *(ak + 3) = x;
	  }
	  else 
	    *(ak +  7) = x;       
	  return;
	}
	else
	  *(ak + 7) =  *(a + i); 
      }
      else {
	i = ((i | 6) << 4) - (i << 3) - 48;
	*(ak + 3) = *(ak + 6);
	position ak2 = a + i;
	if(less(*(ak2), *(ak2 - 8))) i -= 8;
	if(less(*(a + i), x)) {      
	  if(less(*(ak + 6), x)) {
	    *(ak + 3) = *(ak + 1);
	    if(less(*(ak + 1), x)) {
	      *(ak + 1) = *(ak); 
	      if(less(*(ak), x))	
		*(ak) = x;
	      else
		*(ak + 1) = x;	
	    }
	    else
	      *(ak + 3) = x;
	  }
	  else 
	    *(ak + 6) = x;       
	  return;
	}
	else
	  *(ak + 6) =  *(a + i);
      }
    }
    else {
      *(ak + 1) = *(ak + 2);
      if(less(*(ak + 4), *(ak + 5))) {
	i = ((i | 5) << 4) - (i << 3) - 48;
  	*(ak + 2) = *(ak + 5); 
	position ak2 = a + i;
	if(less(*(ak2), *(ak2 - 8))) i -= 8;
	if(less(*(a + i), x)) {      
	  if(less(*(ak + 5), x)) {
	    *(ak + 2) = *(ak + 1);
	    if(less(*(ak + 1), x)) {
	      *(ak + 1) = *(ak); 
	      if(less(*(ak), x))	
		*(ak) = x;
	      else
		*(ak + 1) = x;	
	    }
	    else
	      *(ak + 2) = x;
	  }
	  else 
	    *(ak + 5) = x;       
	  return;
	}
	else
	  *(ak + 5) =  *(a + i);
      }
      else {
	i = ((i | 4) << 4) - (i << 3) - 48;
	*(ak + 2) = *(ak + 4);
	position ak2 = a + i;
	if(less(*(ak2), *(ak2 - 8))) i -= 8;
	if(less(*(a + i), x)) {      
	  if(less(*(ak + 4), x)) {
	    *(ak + 2) = *(ak + 1);
	    if(less(*(ak + 1), x)) {
	      *(ak + 1) = *(ak); 
	      if(less(*(ak), x))	
		*(ak) = x;
	      else
		*(ak + 1) = x;	
	    }
	    else
	      *(ak + 2) = x;
	  }
	  else 
	    *(ak + 4) = x;       
	  return;
	}
	else
	  *(ak + 4) =  *(a + i);
      }
    }      
  }
  j = i | 4;
  if ((((j << 4) - (i << 3) - 56) > p.last_leaf()) && ((i | 7) <= p.last_leaf())) { 
    const position ak = a + i;
    *(ak) = *(ak + 1);
    if(less(*(ak + 2), *(ak + 3))) {
      *(ak + 1) = *(ak + 3);
      if(less(*(ak + 6), *(ak + 7))) {
  	*(ak + 3) = *(ak + 7);
	if(less(*(ak + 7), x)) {
	  *(ak + 3) = *(ak + 1);
	  if(less(*(ak + 1), x)) {
	    *(ak + 1) = *(ak); 
	    if(less(*(ak), x))	
	      *(ak) = x;
	    else
	      *(ak + 1) = x;	
	  }
	  else
	    *(ak + 3) = x;
	}
	else 
	  *(ak +  7) = x;       
      }
      else {
	*(ak + 3) = *(ak + 6);
	if(less(*(ak + 6), x)) {
	  *(ak + 3) = *(ak + 1);
	  if(less(*(ak + 1), x)) {
	    *(ak + 1) = *(ak); 
	    if(less(*(ak), x))	
	      *(ak) = x;
	    else
	      *(ak + 1) = x;	
	  }
	  else
	    *(ak + 3) = x;
	}
	else 
	  *(ak + 6) = x;       
      }
    }
    else {
      *(ak + 1) = *(ak + 2);
      if(less(*(ak + 4), *(ak + 5))) {
	*(ak + 2) = *(ak + 5); 
	if(less(*(ak + 5), x)) {
	  *(ak + 2) = *(ak + 1);
	  if(less(*(ak + 1), x)) {
	    *(ak + 1) = *(ak); 
	    if(less(*(ak), x))	
	      *(ak) = x;
	    else
	      *(ak + 1) = x;	
	  }
	  else
	    *(ak + 2) = x;
	}
	else 
	  *(ak + 5) = x;       
	
      }
      else {
	*(ak + 2) = *(ak + 4);
	if(less(*(ak + 4), x)) {
	  *(ak + 2) = *(ak + 1);
	  if(less(*(ak + 1), x)) {
	    *(ak + 1) = *(ak); 
	    if(less(*(ak), x))	
	      *(ak) = x;
	    else
	      *(ak + 1) = x;	
	  }
	  else
	    *(ak + 2) = x;
	}
	else 
	  *(ak + 4) = x;       
      }
    }
    return;
  }


  while (p.last_child(i) <= p.last_leaf()) {
    j = p.top_all_present(a, i, less);
    if (less(x, *(a + j))) {
      p.update_all_present(a, i, *(a + j), less);
    }
    else {
      p.update_all_present(a, i, x, less);
      return;
    }
    i = j;
  }
  while(p.first_child(i) <= p.last_leaf()) {
    if(p.last_child(i) <= p.last_leaf())
      j = p.top_all_present(a, i, less);	
    else 
      j = p.top_some_absent(a, i, less);
    if (less(x, *(a + j))) {
      p.update_all_present(a, i, *(a + j), less);
      i = j;
    }
    else {
      p.update_all_present(a, i, x, less);
      return;
    }
  }    
  p.update_all_present(a, i, x, less);
  return;      

  //std::cout << "- after sift_down: " << std::endl;
  //show(a, a + p.size());
  //show(p.B.begin(), p.B.end()); 

}

template <int d, typename position, typename index, typename element, 
  typename ordering, typename policy>
void
sift_down2(position a, index i, element x, const ordering& less, policy& p) {
  //std::cout << " - before sift_down i,x: " << i << " " << x << std::endl;
  //show(a, a + p.size());
  //show(p.B.begin(), p.B.end());
  
  index j;
  while (p.last_child(i) <= p.last_leaf()) {
    j = p.top_all_present(a, i,  less);
    if (less(x, *(a + j))) {
      p.update_all_present(a, i, *(a + j), less);
    }
    else {
      p.update_all_present(a, i, x, less);
      return;
    }
    i = j;
  }
  
  while(p.first_child(i) <= p.last_leaf()) {
    if(p.last_child(i) <= p.last_leaf())
      j = p.top_all_present(a, i, less);	
    else 
      j = p.top_some_absent(a, i, less);
    if (less(x, *(a + j))) {
      p.update_all_present(a, i, *(a + j), less);
      i = j;
    }
    else {
      p.update_all_present(a, i, x, less);
      return;
    }
  }    
  p.update_all_present(a, i, x, less);
  return; 

  
  //std::cout << "- after sift_down: " << std::endl;
  //show(a, a + p.size());
  //show(p.B.begin(), p.B.end()); 

} 

#endif // __UTILITY_FUNCTIONS__


